#ifndef __LEG_H__
#define __LEG_H__

#include "motor.h"

typedef struct __Leg_t
{
	/* motors and sensors */
	Motor_t m1;
	Motor_t m2;
	
	/* init param */
	float zero_x;
	float zero_y;
	float zero_angle;
	
	/* ctrl param */
	float x;
	float y;
	float Ax;
	float Ay;
	float max_force;
	
	
} Leg_t;

typedef enum
{
	LEG_INDEX_LF = 0,
	LEG_INDEX_RF = 1,
	LEG_INDEX_LB = 2,
	LEG_INDEX_RB = 3
} Leg_index_e;

extern Leg_t leg[4];

/* 针对弹跳小狗的宏定义 */
#define LF_M1 (leg[LEG_INDEX_LF].m1)
#define LF_M2 (leg[LEG_INDEX_LF].m2)
#define RF_M1 (leg[LEG_INDEX_RF].m1)
#define RF_M2 (leg[LEG_INDEX_RF].m2)
#define LB_M1 (leg[LEG_INDEX_LB].m1)
#define LB_M2 (leg[LEG_INDEX_LB].m2)
#define RB_M1 (leg[LEG_INDEX_RB].m1)
#define RB_M2 (leg[LEG_INDEX_RB].m2)

/* 大小写兼容 */
#define lf_m1 LF_M1
#define lf_m2 LF_M2
#define rf_m1 RF_M1
#define rf_m2 RF_M2
#define lb_m1 LB_M1
#define lb_m2 LB_M2
#define rb_m1 RB_M1
#define rb_m2 RB_M2

/* 腿缩写 */

#define LF leg[LEG_INDEX_LF]
#define RF leg[LEG_INDEX_RF]
#define LB leg[LEG_INDEX_LB]
#define RB leg[LEG_INDEX_RB]


#define PI 3.1415926f
#define RAD_2_DEG(X) ( X / PI * 180.0 )


void Leg_Init(void);
void Leg_SetAngle(Leg_index_e leg_index, float degree1, float degree2);
void Leg_SetPos(Leg_index_e leg_index, float x, float y);
void Leg_SetOutputMax(Leg_t *leg, float spd_max_out, float pos_max_out);
	
#endif
